#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>

class px4control{

public:
    px4control(ros::NodeHandle *NH):nh_(NH){

        //订阅无人机状态话题
	    state_sub = nh_->subscribe<mavros_msgs::State>("mavros/state", 20, boost::bind(&px4control::state_cb, this, _1));

        //订阅无人机实时位置信息
        local_pos_sub = nh_->subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 20, boost::bind(&px4control::local_pos_cb, this, _1));

        //发布无人机多维控制话题
        mavros_setpoint_pos_pub = nh_->advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);   

        //请求无人机解锁服务        
        arming_client = nh_->serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
		
        //请求无人机设置飞行模式
        set_mode_client = nh_->serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    }

    void state_cb(const mavros_msgs::State::ConstPtr& msg);

    void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);

    void Requesting_Offboard();

    void Requesting_Landing();

    void controlvel(geometry_msgs::Twist vel);

    void controlpos(geometry_msgs::PoseStamped pose);

    mavros_msgs::State current_state;
    nav_msgs::Odometry local_pos;
private:
    ros::Subscriber state_sub;
    ros::Subscriber local_pos_sub;
    ros::Publisher  mavros_setpoint_pos_pub;
    ros::Publisher  _pub;
    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;
    ros::NodeHandle *nh_;
};

void px4control::state_cb(const mavros_msgs::State::ConstPtr& msg){
    this->current_state = *msg;
}

void px4control::local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg){
    this->local_pos = *msg;
}

void px4control::Requesting_Offboard() {
    // 请求 OFFBOARD 模式变量
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    
    // 请求解锁变量
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 请求进入 OFFBOARD 模式
    if (current_state.mode != "OFFBOARD") {
        if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
            ROS_INFO("Offboard enabled");
        } else {
            ROS_ERROR("Failed to set OFFBOARD mode");
            // return false;
        }
    }

    // 请求解锁
    if (!current_state.armed) {
        if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
            ROS_INFO("Vehicle armed");
        } else {
            ROS_ERROR("Failed to arm vehicle");
            // return false;
        }
    }

    // return true; // 成功进入 OFFBOARD 模式并解锁
}

void px4control::Requesting_Landing() {
    // 请求 AUTO.LAND 模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "AUTO.LAND";
    
    // 检查是否已经处于 AUTO.LAND 模式
    if (current_state.mode != "AUTO.LAND") {
        if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
            ROS_INFO("AUTO.LAND enabled");
        } else {
            ROS_ERROR("Failed to set AUTO.LAND mode");
            // return false;
        }
    } else {
        ROS_INFO("Already in AUTO.LAND mode");
    }

    // return true; // 已经成功切换或已经处于 AUTO.LAND 模式
}

void px4control::controlpos(geometry_msgs::PoseStamped pose) {
    std::cout << "controlpos" << std::endl;
    std::cout << "pose.x: " << pose.pose.position.x << std::endl;
    std::cout << "pose.y: " << pose.pose.position.y << std::endl;
    std::cout << "pose.z: " << pose.pose.position.z << std::endl;

    mavros_msgs::PositionTarget setpoint_raw;

    // 忽略加速度/力和 yaw 角控制
    setpoint_raw.type_mask = 64 + 128 + 256 + 512;

    // 使用本地 NED 坐标系
    setpoint_raw.coordinate_frame = 1;

    // 动态速度控制（比例控制）
    double error_x = fabs(pose.pose.position.x - this->local_pos.pose.pose.position.x);
    double error_y = fabs(pose.pose.position.y - this->local_pos.pose.pose.position.y);
    double error_z = fabs(pose.pose.position.z - this->local_pos.pose.pose.position.z);

    // 这里设置最大速度为0.05
    double max_vel = 0.05;

    setpoint_raw.velocity.x = error_x > 0.05 ? std::min(error_x * 1, max_vel) : 0;
    setpoint_raw.velocity.y = error_y > 0.05 ? std::min(error_y * 1, max_vel) : 0;
    // setpoint_raw.velocity.z = error_z > 0.05 ? std::min(error_z * 1, max_vel) : 0;

    // 设置目标位置
    setpoint_raw.position.x = pose.pose.position.x;
    setpoint_raw.position.y = pose.pose.position.y;
    setpoint_raw.position.z = pose.pose.position.z;

    // 发布控制指令
    this->mavros_setpoint_pos_pub.publish(setpoint_raw);

}


/*

1        0: 忽略位置 x                
2        1: 忽略位置 y                
4        2: 忽略位置 z               
8        3: 忽略速度 x               
16       4: 忽略速度 y                
32       5: 忽略速度 z                
64       6: 忽略加速度 x              
128      7: 忽略加速度 y              
256      8: 忽略加速度 z               
512      9: 忽略 yaw 角               
1024    10: 忽略 yaw rate（偏航角速度）

*/




